import sys
import time
import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.executors import MultiThreadedExecutor
from semantic_nav_interfaces.action import NavStr
from semantic_nav.action_utils import PriorQueServer


class SemanticNavServer(PriorQueServer):
    def __init__(self, action_name, namespace=''):
        super().__init__(action_type=NavStr, action_name=action_name, namespace=namespace, execute_func=self.execute_func)    

    @staticmethod
    def execute_func(self, goal_handle):
        self.get_logger().info('Executing goal...')
        request = goal_handle.request
        if request.type == request.TYPE_QA:
            return self.execute_QA(goal_handle)
        elif request.type == request.TYPE_NAVIGATION:
            return self.execute_Navigation(goal_handle)
        elif request.type == request.TYPE_DECISION:
            return self.execute_DECISION(goal_handle)

    def execute_QA(self, goal_handle):
        # Start executing the action
        if not self.is_succeeded(goal_handle):
            return NavStr.Result()
        result_msg = NavStr.Result()
        result_msg.result = 'QA result'
        return result_msg

    def execute_NAVIGATION(self, goal_handl):
        pass

    def execute_DECISION(self, goal_handle):
        count = 0
        rate = self.create_rate(self.sample_rate)
        while rclpy.ok():
            # check if a cancel request has been received
            if self.is_interrupted(goal_handle):
                return NavStr.Result()
            feedback_msg = NavStr.Feedback()
            feedback_msg.feedback = "DECISION"
            # Publish the feedback
            goal_handle.publish_feedback(feedback_msg)
            # success condition
            if count > 5:
                if not self.is_succeeded(goal_handle):
                    return NavStr.Result()
                result_msg = NavStr.Result()
                result_msg.result = 'DECISION result'
                return result_msg
            count += 1
            rate.sleep()


def main(args=None):
    rclpy.init(args=args)
    try:
        semantic_nav_server = SemanticNavServer(action_name='semantic_nav')
        executor = MultiThreadedExecutor()
        rclpy.spin(semantic_nav_server, executor=executor)
    except KeyboardInterrupt:
        pass
    except ExternalShutdownException:
        sys.exit(1)


if __name__ == '__main__':
    main()
